#pragma config(Sensor, dgtl1,  EncoderRight,        sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  EncoderLeft,         sensorQuadEncoder)
#pragma config(Motor,  port2,           RightDrive,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           LeftDrive,     tmotorNormal, openLoop)
#pragma config(Motor,  port4,           LeftArm1,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           RightArm,      tmotorNormal, openLoop)
#pragma config(Motor,  port6,           RightTread,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           LeftArm2,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port8,           LeftTread,     tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
#include "Encoder.c"                    //Autonomous Encoder control
#define MAX_ARM 110
#define MAX_TREAD 127
/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
  // .....................................................................................
  // Insert user code here.
  // .....................................................................................
  wait10Msec(50);
  motor[RightDrive] = 90;
  motor[LeftDrive] = 90;
  motor[RightArm] = 127;
  motor[LeftArm1] = 127;
  motor[LeftArm2] = 127;
  wait10Msec(80);
  motor[RightDrive] = 0;
  motor[LeftDrive] = 0;
  motor[RightArm] = 15;
  motor[LeftArm1] = 15;
  motor[LeftArm2] = 15;
  \
  motor[RightTread] = 100;
  motor[LeftTread] = 100;
  wait10Msec(400);
  motor[RightDrive] = -90;
  motor[LeftDrive] = -90;
  motor[RightArm] = 0;
  motor[LeftArm1] = 0;
  motor[LeftArm2] = 0;
  wait10Msec(60);
  motor[RightDrive] = 0;
  motor[LeftDrive] = 0;
  motor[LeftTread] = 0;
  motor[RightTread] = 0;



}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////


void tankDrive()
{
  motor[RightDrive] = vexRT[Ch2];
  motor[LeftDrive] = vexRT[Ch3];
}

void armDrive()
{
  if(vexRT[Btn5U] == 1)
  {
    motor[RightArm] = MAX_ARM;
    motor[LeftArm1] = MAX_ARM;
    motor[LeftArm2] = MAX_ARM;
  }
  else if(vexRT[Btn5D] == 1)
  {
    motor[LeftArm1] = -MAX_ARM;
    motor[LeftArm2] = -MAX_ARM;
    motor[RightArm] = -MAX_ARM;

  }
  else
  {
    motor[LeftArm1] = 0;
    motor[LeftArm2] = 0;
    motor[RightArm] = 0;
  }
}

void TreadDrive()
{
  if(vexRT[Btn8D] + vexRT[Btn8U] + vexRT[Btn8L] + vexRT[Btn8R] == 0)
  {
    motor[RightTread] = motor[LeftTread] = (vexRT[Btn6D] - vexRT[Btn6U])*MAX_TREAD;
  }
  else
  {
    motor[RightTread] = (vexRT[Btn8U] -vexRT[Btn8R])*MAX_TREAD;
    motor[LeftTread] = (vexRT[Btn8L] -vexRT[Btn8D])*MAX_TREAD;
  }
}

task usercontrol()
{
  while (true)
  {
    tankDrive();
    armDrive();
    TreadDrive();
  }
}
